#include "filter.h"

adc_filter_param adc1_filter_ia;
adc_filter_param adc1_filter_ib;
adc_filter_param adc1_filter_ic;
adc_filter_param adc1_filter_ua;
adc_filter_param adc1_filter_ub;
adc_filter_param adc1_filter_uc;

void ADC_One_Filter(uint16_t adc_value, adc_filter_param *adc_filter)
{
    uint8_t i;
    
    adc_filter->in = adc_value;
    adc_filter->out = 0;
    
    for(i=0;i<LEN_adc;i++) 
    {
        if(i<(LEN_adc-1))
            adc_filter->filter_buf[i] = adc_filter->filter_buf[i+1];
        else adc_filter->filter_buf[i] = adc_value;
        adc_filter->out += adc_filter->filter_buf[i];
    }
    adc_filter->out /= LEN_adc;

    adc_filter->out = adc_filter->out; 
}

void ADC_Three_Filter(uint16_t adc_value, uint16_t adc_value1, uint16_t adc_value2, adc_filter_param *adc_filter, adc_filter_param *adc_filter1, adc_filter_param *adc_filter2)
{
    uint8_t i;
    
    adc_filter->in = adc_value;
    adc_filter1->in = adc_value1;
    adc_filter2->in = adc_value2;
    
    adc_filter->out = 0;
    adc_filter1->out = 0;
    adc_filter2->out = 0;
    
    for(i=0;i<LEN_adc;i++) 
    {
        if(i<(LEN_adc-1))
        {
            adc_filter->filter_buf[i] = adc_filter->filter_buf[i+1];
            adc_filter1->filter_buf[i] = adc_filter1->filter_buf[i+1];
            adc_filter2->filter_buf[i] = adc_filter2->filter_buf[i+1];
        }
        else 
        {
            adc_filter->filter_buf[i] = adc_value;
            adc_filter1->filter_buf[i] = adc_value1;
            adc_filter2->filter_buf[i] = adc_value2;
        }
        adc_filter->out += adc_filter->filter_buf[i];
        adc_filter1->out += adc_filter1->filter_buf[i];
        adc_filter2->out += adc_filter2->filter_buf[i];
    }
    adc_filter->out /= LEN_adc;
    adc_filter1->out /= LEN_adc;
    adc_filter2->out /= LEN_adc;
}


speed_filter_param_f32 speed_filter_L;
speed_filter_param_f32 speed_filter_R;

void Speed_One_Filter(float speed_value, speed_filter_param_f32 *speed_filter)
{
    uint8_t i;
    speed_filter->in = speed_value;
    speed_filter->out = 0;
    for(i=0;i<LEN_speed;i++) 
    {
        if(i<(LEN_speed-1))
            speed_filter->filter_buf[i] = speed_filter->filter_buf[i+1];
        else speed_filter->filter_buf[i] = speed_value;
        speed_filter->out += speed_filter->filter_buf[i];
    }
    
    speed_filter->out /= LEN_speed;


}
const float NUM_3[4] = {0.001567010302, 0.004701030906, 0.004701030906, 0.001567010302};
const float DEN_3[4] = {1,   -2.498608351,    2.115254164,  -0.6041097045};

float iir_filter_3(float x, unsigned int ch){
	static float y3,x3_n1[MAX_FILTER_CH_3],x3_n2[MAX_FILTER_CH_3],x3_n3[MAX_FILTER_CH_3]
					,y3_n1[MAX_FILTER_CH_3],y3_n2[MAX_FILTER_CH_3],y3_n3[MAX_FILTER_CH_3];
	y3 = NUM_3[0]*x + NUM_3[1]*x3_n1[ch] + NUM_3[2]*x3_n2[ch] + NUM_3[3]*x3_n3[ch]
						- DEN_3[1]*y3_n1[ch] - DEN_3[2]*y3_n2[ch] - DEN_3[3]*y3_n3[ch];
	//if(y<EPS && y>-EPS) y = 0.0;
	y3_n3[ch]=y3_n2[ch]; y3_n2[ch]=y3_n1[ch]; y3_n1[ch]=y3;
	x3_n3[ch]=x3_n2[ch]; x3_n2[ch]=x3_n1[ch]; x3_n1[ch]=x;
	return y3;
}
